import open3d as o3d
from multiprocessing import Pool
from multiprocessing.pool import ThreadPool
import numpy as np
from PyQt5.QtWidgets import QMessageBox
from volume_groundequ import GroundEqu

from manual_registration import pick_points


class Volume:
    def __init__(self):
        self.equ=""
        self.all=""
        self.help = '''
            (1)选择要计算的电云文件，点云以ply结尾，注意计算体积需要经过裁剪，去除地面无关物品；
            (2)程序开始前会让您在点云中选择至少9个地面点，地面模拟会直接影响体积计算精度。
            （3）shift+鼠标左击是选择，shift+鼠标右击是取消选择。选中后屏幕会显示一个小球。
            (4)至少选择3个地面点后，按q退出，选择共同点数量越多越好",
            '''
    def filter_obj_by_groundequ(self,thresh):
        a, b, c,d =self.equ
        vec3 = np.array([a, b, c])
        down = np.sqrt(np.sum(np.multiply(vec3, vec3)))
        rows = self.all.shape[0]
        vec1 = np.repeat([[a, b, c, d]], rows, axis=0)

        vec2 = np.ones((rows, 4))
        vec2[:, 0:3] = self.all
        res1 = np.multiply(vec1, vec2)
        up = np.abs(np.sum(res1, axis=1))
        dis = up / down
        # mask = dis >= self.threshhold
        print ("before",len(dis))
        mask = dis >= thresh
        obj = self.all[mask]
        print ('after',len(obj))
        return obj.tolist()
    def project_point2_plane(self,pnt,a,b,c,d):

# 直线朝着地平面做垂线，和平面的交点
        right=np.array([
            [1/a,-1/b,0],
            [1/a,0, -1/c],
            [a, b, c],
        ])
        x,y,z=pnt
        left=np.array([x/a-y/b,x/a-z/c,-d])
        res=np.linalg.solve(right,left)
        return res.tolist()



    def signed_dis_2ground(self,src):
        a, b, c,d = self.equ
        x1,y1,z1=src
        up=a*x1+b*y1+c*z1+d
        down=np.sqrt(a*a+b*b+c*c)
        return up/down

    def connnect_src_projection(self,src):
        res=[]
        a, b, c,d = self.equ
        down=np.sqrt(a*a+b*b+c*c)
        x1,y1,z1=src
        signed_dis=self.signed_dis_2ground(src)
        n=10
        step=signed_dis/n
        lines=[step*i for i in range(n)]
        for i in lines:
            # 和地面平行的平面一个一个挪动,求和法线交点
            d1=i*down-(a*x1+b*y1+c*z1)
            projection=self.project_point2_plane(src,a,b,c,d1)
            res.append(projection)
        return res

    def volume_from_convex_hull(self,pcd):
        mesh =pcd.compute_convex_hull()
        center = mesh.get_center()
        vers = np.asarray(mesh.vertices)
        tris = np.asarray(mesh.triangles)
        sum = 0
        for i in range(tris.shape[0]):
            a, b, c = tris[i]
            ae = vers[a] - center
            be = vers[b] - center
            ce = vers[c] - center
            cross = np.cross(ae, be)
            s = np.linalg.norm(cross) / 2
            h = np.dot(cross, ce) / np.linalg.norm(cross)
            vol = np.abs(s * h / 3)
            sum = vol + sum
        print("my sum",sum)
        return sum


    def main(self,file):
        pnt = o3d.io.read_point_cloud(file)
        idx = pick_points(pnt)
        if len(idx) < 3: return
        self.all = np.asarray(pnt.points)
        ground = self.all[idx]
        self.equ = GroundEqu().main(ground)

        obj=self.filter_obj_by_groundequ(0.15)
        with ThreadPool() as p:
            res=p.map(self.connnect_src_projection,obj)

        res=np.asarray(res)
        res1=np.concatenate(res)

        pcd = o3d.geometry.PointCloud()
        pcd.points = o3d.utility.Vector3dVector(res1)
        vol=self.volume_from_convex_hull(pcd)
        o3d.visualization.draw_geometries([pcd])

        return vol

if __name__ == "__main__":
    xyz=np.asarray([
        [1,2,3],
        [4, 6,9],
        [1, 0, 3],
        [1, 9, 3],
    ])
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(xyz)
    # o3d.io.write_point_cloud("./process/temp_vol.ply", pcd)
    mesh = pcd.compute_convex_hull()

